#include <cmath>
#include "Vtl/geometry.h"

namespace Vtl
{
	namespace angles
	{
		template <typename Scalar>
		inline Scalar normAngle(const Scalar &_Alpha)
		{
			return (_Alpha >= 0  ?  fmod(_Alpha+Scalar(M_PI), Scalar(2.0*M_PI))-Scalar(M_PI) : -(fmod(Scalar(M_PI)-_Alpha, Scalar(2.0*M_PI))-Scalar(M_PI)));
		}

		template <typename Scalar>
		inline Scalar rad2deg(const Scalar &_Alpha)
		{
			return _Alpha*Scalar(57.295779513082320876798154814105);
		}

		template <typename Scalar>
		inline Scalar deg2rad(const Scalar &_Alpha)
		{
			return _Alpha*Scalar(0.01745329251994329576923690768489);
		}

		template <typename Scalar, int Dimension>
		inline Scalar getAngle(const Eigen::Matrix<Scalar,Dimension,1> &_V1,
			const Eigen::Matrix<Scalar,Dimension,1> &_V2)
		{
			// Compute the actual angle
			Scalar rad = Scalar(_V1.dot(_V2) / sqrtl(_V1.squaredNorm() * _V2.squaredNorm()));
			if (rad < -1.0) rad = -1.0;
			if (rad >  1.0) rad = 1.0;
			return acosl(rad);
		}

		template <typename Scalar, int Dimension>
		inline Scalar getDirection(const Eigen::Matrix<Scalar,Dimension,1> &_Vect)
		{
			Scalar rad = atan2l(_Vect(0),_Vect(1));
			return rad;
		}

		template <typename Scalar, int Dimension>
		inline Scalar getElevation(const Eigen::Matrix<Scalar,Dimension,1> &_Vect)
		{
			Scalar i = _Vect(0);
			Scalar j = _Vect(1);
			Scalar k = _Vect(2);
			Scalar rad = (k>=Scalar(0)?1:-1)*atanl(sqrtl((k*k)/((i*i)+(j*j))));
			return rad;
		}
	};
}